#include "Cube.h"
#include <iostream>

#define __CL_ENABLE_EXCEPTIONS
#define __NO_STD_VECTOR
#include <CL/cl.hpp>

extern float particleRadius;

extern cl_float* vertexData;
extern cl_float* normalData;
extern cl_float* colorData;

extern int vertexCount;


float Cube::side = particleRadius * 3.0f;

float Cube::normals[6][3] = { /* Cube face normals */
	{ -1.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f }, { 1.0f, 0.0f, 0.0f },
       	{ 0.0f, -1.0f, 0.0f }, { 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, -1.0f } };
int Cube::faceIndices[6][4] = { /* Cube faces' vertex indices */
	{ 0, 1, 2, 3 }, { 3, 2, 6, 7 }, { 7, 6, 5, 4 },
       	{ 4, 5, 1, 0 }, { 5, 6, 2, 1 }, { 7, 4, 0, 3 } };
float Cube::vertices[8][3] = { /* Cube vertex positions */
	{ -side, -side, side }, { -side, -side, -side }, { -side, side, -side },
	{ -side, side, side }, { side, -side, side }, { side, -side, -side },
	{ side, side, -side }, { side, side, side } };

Cube::Cube() {
	initialInverseInertiaTensorDiagonal[0] = initialInverseInertiaTensorDiagonal[1] =
		initialInverseInertiaTensorDiagonal[2] = 6.0f / (mass*4*side*side);

	float x,y,z;
	x = (count % 2) * 1.9f * side;
	y = count * 3.0f * side;
	z = ((count % 4)/2) * 1.9f * side;
	position = {x, y, z};

	numberOfParticles = 27;
	createParticles();
}

Cube::Cube(float* position) {
	initialInverseInertiaTensorDiagonal[0] = initialInverseInertiaTensorDiagonal[1] =
		initialInverseInertiaTensorDiagonal[2] = 6.0f / (mass*4*side*side);

	this->position[0] = position[0];
	this->position[1] = position[1];
	this->position[2] = position[2];

	numberOfParticles = 27;
	createParticles();
}

void Cube::createParticles() {
	particles = new Particle*[numberOfParticles];
	int i=0;
	float space = particleRadius*2.0f;
	for (int j=-1; j<=1; j++) {
		for (int k=-1; k<=1; k++) {
			for (int l=-1; l<=1; l++) {
				float x = position[0]+space*j;
				float y = position[1]+space*k;
				float z = position[2]+space*l;
				float particlePos[3] = {x,y,z};
				particles[i] = new Particle(particlePos, mass/numberOfParticles);
				i++;
			}
		}
	}
}

void Cube::applyRotationToParticles() {
	int i=0;
	float space = particleRadius*2.0f;
	for (int j=-1; j<=1; j++) {
		for (int k=-1; k<=1; k++) {
			for (int l=-1; l<=1; l++) {
				float x = space*j;
				float y = space*k;
				float z = space*l;
				float originalRelativePos[3] = {x, y, z};

				particles[i]->applyRotation(rotationMatrix, originalRelativePos, position);
				i++;
			}
		}
	}
}

void Cube::updateVBO(int vertexIndex) {

	for (int i=0; i<6; i++) { //for every face
		for (int j=0; j<4; j++) { //for every vertex in the face

			float r0 = rotationMatrix[0];
			float r1 = rotationMatrix[1];
			float r2 = rotationMatrix[2];
			float r3 = rotationMatrix[3];
			float r4 = rotationMatrix[4];
			float r5 = rotationMatrix[5];
			float r6 = rotationMatrix[6];
			float r7 = rotationMatrix[7];
			float r8 = rotationMatrix[8];

			float v0 = vertices[faceIndices[i][j]][0];
			float v1 = vertices[faceIndices[i][j]][1];
			float v2 = vertices[faceIndices[i][j]][2];

			vertexData[vertexIndex] = r0*v0 + r1*v1 + r2*v2 + position[0];
			vertexData[vertexIndex+1] = r3*v0 + r4*v1 + r5*v2 + position[1];
			vertexData[vertexIndex+2] = r6*v0 + r7*v1 + r8*v2 + position[2];

			float n0 = normals[i][0];
			float n1 = normals[i][1];
			float n2 = normals[i][2];

			normalData[vertexIndex] = r0*n0 + r1*n1 + r2*n2;
			normalData[vertexIndex+1] = r3*n0 + r4*n1 + r5*n2;
			normalData[vertexIndex+2] = r6*n0 + r7*n1 + r8*n2;

			vertexIndex += 3;
		}
	}
}

void Cube::updateColorArray(int vertexIndex) {
	for (int i=0; i<24; i++) {
		colorData[vertexIndex] = color[0];
		colorData[vertexIndex+1] = color[1];
		colorData[vertexIndex+2] = color[2];

		vertexIndex += 3;
	}
}

void Cube::updateInverseInertiaTensor() {
	float a = rotationMatrix[0];
	float b = rotationMatrix[1];
	float c = rotationMatrix[2];
	float d = rotationMatrix[3];
	float e = rotationMatrix[4];
	float f = rotationMatrix[5];
	float g = rotationMatrix[6];
	float h = rotationMatrix[7];
	float i = rotationMatrix[8];

	float u = initialInverseInertiaTensorDiagonal[0];
	float v = initialInverseInertiaTensorDiagonal[1];
	float w = initialInverseInertiaTensorDiagonal[2];

	inverseInertiaTensor[0] = u*a*a + b*b*v + c*c*w;
	inverseInertiaTensor[1] = a*d*u + b*e*v + c*f*w;
	inverseInertiaTensor[2] = a*g*u + b*h*v + c*i*w;
	inverseInertiaTensor[3] = a*d*u + b*e*v + c*f*w;
	inverseInertiaTensor[4] = u*d*d + e*e*v + f*f*w;
	inverseInertiaTensor[5] = d*g*u + e*h*v + f*i*w;
	inverseInertiaTensor[6] = a*g*u + b*h*v + c*i*w;
	inverseInertiaTensor[7] = d*g*u + e*h*v + f*i*w;
	inverseInertiaTensor[8] = u*g*g + h*h*v + i*i*w;
}
